#ifndef FILTER_HPP
#define FILTER_HPP

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/filters/voxel_grid.h>
#include <mutex>
#include <deque>

struct FilterParams {
  
        float left_up_y;
        float left_up_z;
        float right_down_y;
        float right_down_z;
   
};

class PointCloudFilter {
public:
    PointCloudFilter(){
        input_cloud_ = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>());
        filtered_cloud_ = pcl::PointCloud<pcl::PointXYZI>::Ptr(new pcl::PointCloud<pcl::PointXYZI>());
    };
    ~PointCloudFilter(){};

    void setInputCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud);
    void filterCloud(int n);
    pcl::PointCloud<pcl::PointXYZI>::Ptr getFilteredCloud();
    void voxelFilter(void)
    {
            double leaf_size=0.01;
            pcl::VoxelGrid<pcl::PointXYZI> voxel_filter;
            voxel_filter.setInputCloud(filtered_cloud_);
            voxel_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
            voxel_filter.filter(*filtered_cloud_);
            //return output_cloud;
        }
    FilterParams filter_small_0, filter_small_1,filter_small_2;
    int filter_mode=0; //滤波模式 0-小目标 1-中目标 2-大目标（井盖）
private:
    pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud_;
    pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_cloud_;
    std::mutex cloud_mutex_;
    std::deque<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloud_queue_;
};
void PointCloudFilter::setInputCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {
   // std::lock_guard<std::mutex> lock(cloud_mutex_);
    input_cloud_ = cloud;
}
void PointCloudFilter::filterCloud(int n) {
    // Filtering logic goes here
    if (!input_cloud_) {
        filtered_cloud_->clear();
        return;
    }
    filtered_cloud_->clear();
    // Apply filtering based on the parameters
    //filtered_cloud_ = pcl::PointCloud<pcl::PointXYZI::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
    if(filter_mode==0) //小目标滤波
    {
        for (const auto& point : input_cloud_->points) {
            if (point.y < filter_small_0.left_up_y && point.y > filter_small_0.right_down_y &&
                point.z < filter_small_0.left_up_z && point.z > filter_small_0.right_down_z) {
                filtered_cloud_->points.push_back(point);
            }
        }
    }
    if(filter_mode==1) //中目标滤波
    {
        for (const auto& point : input_cloud_->points) {
            if (point.y < filter_small_1.left_up_y && point.y > filter_small_1.right_down_y &&
                point.z < filter_small_1.left_up_z && point.z > filter_small_1.right_down_z) {
                filtered_cloud_->points.push_back(point);
            }
        }
    }
    if(filter_mode==2) //大目标滤波
    {
        for (const auto& point : input_cloud_->points) {
            if (point.y < filter_small_2.left_up_y && point.y > filter_small_2.right_down_y &&
                point.z < filter_small_2.left_up_z && point.z > filter_small_2.right_down_z) {
                filtered_cloud_->points.push_back(point);
            }
        }
    }
    filtered_cloud_->width = filtered_cloud_->points.size();
    filtered_cloud_->height = 1;
    filtered_cloud_->is_dense = true;
}

pcl::PointCloud<pcl::PointXYZI>::Ptr PointCloudFilter::getFilteredCloud() {
   // std::lock_guard<std::mutex> lock(cloud_mutex_);
    return filtered_cloud_;
}

#endif
